clear;
clc;
close;
[XYZ,TOA,base_station_num,terminial_num,dimension_num]= read_txt('..\testcase_data_ans\sample_case001_input.txt');
% 将TOA 矩阵元素转成  转成距离
% 无线电信号的传播速度统一取〖3×10^8〗m/s
% 假设TOA矩阵单位为s
speed = 3*10^8;
TOA_distance = TOA.*speed;
% 计算实际坐标
syms x y z;
% 存储最后所以等式的数组
res = zeros(terminial_num,3);
all_fomula  = sym ([1:terminial_num]);
for j  = [1: terminial_num]
    termial_tmp=TOA_distance(j,:);%计算 TOA_distance(j,:); 的实际坐标,是个数组
    % 遍历所有的基站
    for i = [1:base_station_num]
         % 圆心
        x1 = XYZ(i,1);
        y1 = XYZ(i,2);
        z1 = XYZ(i,3);
        r = termial_tmp(i);
        % 球体公式 == (x-x1)^2 + (y-y1)^2 + (z-z1)^2  = r^2
        all_fomula(i) = (x-x1)^2 + (y-y1)^2 + (z-z1)^2  == r^2;
    end
    [x_res,y_res,z_res]=solve(all_fomula);% :TODO 这个得不到交点！  思路对吗？  
%     res(j,:) = double([x_res,y_res,z_res]);% 这里出错
    break;
end



